package jp.seraph.robocup.soccer.sample.agent.turn;

import jp.seraph.jsade.model.nao.NaoJointIdentifier;
import jp.seraph.jspf.pose.DefaultPose;
import jp.seraph.jspf.pose.Pose;

public class TurnPoseCreator {
    public static Pose createDefaultPose(){
        DefaultPose tResult = new DefaultPose();
        tResult.setJointAngle(NaoJointIdentifier.RAJ1, -90);
        tResult.setJointAngle(NaoJointIdentifier.RAJ3, 90);
        tResult.setJointAngle(NaoJointIdentifier.RAJ4, 10);

        tResult.setJointAngle(NaoJointIdentifier.LAJ1, -90);
        tResult.setJointAngle(NaoJointIdentifier.LAJ3, -90);
        tResult.setJointAngle(NaoJointIdentifier.LAJ4, -10);


        tResult.setJointAngle(NaoJointIdentifier.RLJ2, 0);
        tResult.setJointAngle(NaoJointIdentifier.RLJ3, 20);
        tResult.setJointAngle(NaoJointIdentifier.RLJ4, -30);
        tResult.setJointAngle(NaoJointIdentifier.RLJ5, 15);
        tResult.setJointAngle(NaoJointIdentifier.RLJ6, 0);

        tResult.setJointAngle(NaoJointIdentifier.LLJ2, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ3, 20);
        tResult.setJointAngle(NaoJointIdentifier.LLJ4, -30);
        tResult.setJointAngle(NaoJointIdentifier.LLJ5, 15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ6, 0);


        return tResult;
    }

    /**
     * 最初の足を上げるために、その逆の足に重心を移す。
     *
     * @return
     */
    public static Pose createTurnPose1(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ6, 15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ6, 15);
        tResult.setJointAngle(NaoJointIdentifier.RLJ2, -15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ2, -15);

        return tResult;
    }

    /**
     * 足を上げる
     * @return
     */
    public static Pose createTurnPose2(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ3, 35);
        tResult.setJointAngle(NaoJointIdentifier.RLJ4, -45);

        return tResult;
    }

    /**
     * 股関節を外向きに回転させる
     * @return
     */
    public static Pose createTurnPose3(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ1, -45);
        tResult.setJointAngle(NaoJointIdentifier.LLJ1, -15);

        return tResult;
    }

    /**
     * 足を下ろすと同時に、重心を戻す
     * @return
     */
    public static Pose createTurnPose4(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ3, 0);
        tResult.setJointAngle(NaoJointIdentifier.RLJ4, -40);
        tResult.setJointAngle(NaoJointIdentifier.RLJ5, 30);


        tResult.setJointAngle(NaoJointIdentifier.RLJ6, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ6, 0);
        tResult.setJointAngle(NaoJointIdentifier.RLJ2, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ2, 0);

        return tResult;
    }

    /**
     * 2番目の足を上げるために重心を逆側に移す
     * @return
     */
    public static Pose createTurnPose5(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ6, -15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ6, -15);
        tResult.setJointAngle(NaoJointIdentifier.RLJ2, 15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ2, 15);


        return tResult;
    }

    /**
     * 番目の足を上げる
     *
     * @return
     */
    public static Pose createTurnPose6(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.LLJ3, 45);
        tResult.setJointAngle(NaoJointIdentifier.LLJ4, -60);

        return tResult;
    }

    /**
     * 股関節を内向きに回転させる
     * 最初に動かした方の足は完全には元に戻さない。
     * @return
     */
    public static Pose createTurnPose7(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.RLJ1, -15);
        tResult.setJointAngle(NaoJointIdentifier.LLJ1, 0);

        return tResult;
    }

    /**
     * 足を下ろす
     * case 7 で戻しきっていなかった分戻す
     * case 4 で足を手前に下ろしていた分を戻す。
     * 重心を元に戻す。
     * @return
     */
    public static Pose createTurnPose8(){
        DefaultPose tResult = new DefaultPose();

        tResult.setJointAngle(NaoJointIdentifier.LLJ1, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ3, 20);
        tResult.setJointAngle(NaoJointIdentifier.LLJ4, -30);

        tResult.setJointAngle(NaoJointIdentifier.RLJ1, 0);

        tResult.setJointAngle(NaoJointIdentifier.RLJ3, 20);
        tResult.setJointAngle(NaoJointIdentifier.RLJ4, -30);
        tResult.setJointAngle(NaoJointIdentifier.RLJ5, 15);

        tResult.setJointAngle(NaoJointIdentifier.RLJ6, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ6, 0);
        tResult.setJointAngle(NaoJointIdentifier.RLJ2, 0);
        tResult.setJointAngle(NaoJointIdentifier.LLJ2, 0);

        return tResult;
    }
}
